#include "zdt_motor.hpp"
#include <glog/logging.h>

zdt_motor::zdt_motor(uint8_t id, rs485_v3 *rs485, agile_modbus_rtu_master_simple *modbus) : _id(id),
                                                                                            _rs485(rs485),
                                                                                            _modbus(modbus)

{
    if (_rs485->rs485_get_flag() != YJ_OK)
    {
        _rs485->rs485_open();
    }

    if (_rs485->rs485_get_flag() != YJ_OK)
    {
        _rs485_flag = YJ_ERROR;
        return;
    }

    if (_modbus->init() != 0)
    {
        _rs485_flag = YJ_ERROR;
        return;
    }
    _des_pos_at.store(0, std::memory_order_release);
    _rs485_flag = YJ_OK;
}

inline yj_return zdt_motor::_read_input_reg(uint32_t reg_addr, std::vector<uint16_t> &reg_data)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "_read_input_reg: zdt_motor init error";
        return YJ_ERROR;
    }

    if (_modbus->read_input_register_multiple(_id, reg_addr, reg_data) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::read_speed(int32_t &speed)
{
    std::vector<uint16_t> speed_temp(2);
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "read_speed: zdt_motor init error";
        return YJ_ERROR;
    }
    if (_read_input_reg(0x35, speed_temp) != YJ_OK)
    {
        return YJ_ERROR;
    }

    speed = speed_temp[1];
    if (speed_temp[0] == 0x01)
        speed = -1 * speed;

    return YJ_OK;
}

yj_return zdt_motor::read_position(double &position)
{
    std::vector<uint16_t> pos_temp(3);
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "read_position: zdt_motor init error";
        return YJ_ERROR;
    }
    if (_read_input_reg(0x36, pos_temp) != YJ_OK)
    {
        return YJ_ERROR;
    }

    position = static_cast<double>(pos_temp[1] << 16 | pos_temp[2]);
    if (pos_temp[0] == 0x01)
        position = -1 * position;
    position = position * 360 / 65536;

    return YJ_OK;
}

yj_return zdt_motor::set_speed(
    int32_t speed,
    uint8_t accel,
    bool is_sync)
{

    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_speed: zdt_motor init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> speed_temp(3);

    if (speed < 0)
    {
        speed_temp[0] = (0x01 << 8) | accel;
        speed_temp[1] = -1 * speed;
    }
    else
    {
        speed_temp[0] = (0x00 << 8) | accel;
        speed_temp[1] = speed;
    }

    speed_temp[2] = 0;
    if (is_sync)
        speed_temp[2] = 0x01 << 8;

    if (speed_temp[1] > 3000)
    {
        LOG(WARNING) << "speed is too fast " << speed_temp[1] << ", set to 3000";
        speed_temp[1] = 3000;
    }

    if (_modbus->write_holding_register_multiple(_id, 0xF6, speed_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::set_position(
    int32_t speed,
    uint8_t accel,
    double circle_num,
    bool is_sync,
    motor_pos_mode_en pos_mode,
    bool is_waiting)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_position: zdt_motor init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> pos_temp(5);

    if (speed < 0)
    {
        pos_temp[0] = (0x01 << 8) | accel;
        pos_temp[1] = -1 * speed;
        _des_pos_at.store(-1 * circle_num * 360, std::memory_order_release);
    }
    else
    {
        pos_temp[0] = (0x00 << 8) | accel;
        pos_temp[1] = speed;
        _des_pos_at.store(circle_num * 360, std::memory_order_release);

    }
    
        
    
    uint32_t pulse_num = circle_num * 200 * 256;
    pos_temp[2] = pulse_num >> 16;
    pos_temp[3] = pulse_num & 0xFFFF;

    pos_temp[4] = 0;
    if (pos_mode == motor_pos_mode_absolute)
        pos_temp[4] |= 0x01 << 8;

    if (is_sync)
        pos_temp[4] |= 0x01;

    if (pos_temp[1] > 3000)
    {
        LOG(WARNING) << "speed is too fast " << pos_temp[1] << ", set to 3000";
        pos_temp[1] = 3000;
    }

    if (_modbus->write_holding_register_multiple(_id, 0xFD, pos_temp) != 0)
    {
        return YJ_ERROR;
    }

    if (is_waiting)
    {
        int retry = 0;
        bool is_pos_ok = false;
        while (retry < 2)
        {
            if (read_is_pos_ok(is_pos_ok) != YJ_OK)
                retry++;

            if (is_pos_ok)
                break;

            std::this_thread::sleep_for(std::chrono::milliseconds(10));
        }

        if (retry >= 2)
        {
            LOG(ERROR) << "set_position: read is_pos_ok failed";
            return YJ_ERROR;
        }

        if (!is_pos_ok)
        {
            return YJ_ERROR;
        }
    }

    return YJ_OK;
}

yj_return zdt_motor::start_sync(bool is_broadcast)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_speed: zdt_motor init error";
        return YJ_ERROR;
    }

    uint8_t send_id = _id;
    if (is_broadcast)
    {
        send_id = 0;
    }

    std::vector<uint16_t> data_temp(1);
    data_temp[0] = 0x6600;

    if (_modbus->write_holding_register_multiple(send_id, 0xFF, data_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::stop_motor(bool is_sync, bool is_broadcast)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_speed: zdt_motor init error";
        return YJ_ERROR;
    }

    uint8_t send_id = _id;
    if (is_broadcast)
    {
        send_id = 0;
    }

    std::vector<uint16_t> data_temp(1);
    data_temp[0] = 0x9800;

    if (is_sync)
    {
        data_temp[0] |= 0x01;
    }

    if (_modbus->write_holding_register_multiple(send_id, 0xFE, data_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::clear_pos()
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "clear_pos: zdt_motor init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> data_temp(1);
    data_temp[0] = 0x01;

    if (_modbus->write_holding_register_single(_id, 0x0A, data_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::read_status(motor_status_t &status)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "read_status: zdt_motor init error";
        return YJ_ERROR;
    }
    std::vector<uint16_t> speed_temp(2);

    if (_modbus->read_input_register_multiple(_id, 0x35, speed_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::set_zero(motor_zero_mode_en mode, bool is_sync)
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "set_zero: zdt_motor init error";
        return YJ_ERROR;
    }
    std::vector<uint16_t> data_temp(1);
    data_temp[0] = static_cast<uint16_t>(mode) << 8;
    data_temp[0] |= static_cast<uint16_t>(is_sync);

    if (_modbus->write_holding_register_multiple(_id, 0x9A, data_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::stop_zero()
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "stop_zero: zdt_motor init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> data_temp(1);
    data_temp[0] = 0x4800;

    if (_modbus->write_holding_register_multiple(_id, 0x9C, data_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}

yj_return zdt_motor::read_zero_status(bool &is_zero, bool &is_zero_ok)
{
    std::vector<uint16_t> zero_temp(1);
    if (_read_input_reg(0x3B, zero_temp) != YJ_OK)
    {
        return YJ_ERROR;
    }

    if ((zero_temp[0] & 0x08) > 0) // 回零是否正常位
    {
        is_zero_ok = false;
    }
    else
    {
        is_zero_ok = true;
    }

    if ((zero_temp[0] & 0x04) > 0) // 是否在回零
    {
        is_zero = true;
    }
    else
    {
        is_zero = false;
    }

    // LOG(INFO) << "read_zero_status: is_zero_ok = " << is_zero_ok << ", is_zero = " << is_zero;

    return YJ_OK;
}

yj_return zdt_motor::read_is_pos_ok( double death_zone_angle, uint64_t timeout_ms)
{

    double pos;

    uint64_t in_time = yj_time_get_millitimestamp();
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    while (yj_time_get_millitimestamp() - in_time < timeout_ms) 
    {
        read_position(pos) ;

        double des_pos_temp = _des_pos_at.load(std::memory_order_acquire);
        double temp  = fabs(des_pos_temp) - fabs(pos);
        if (fabs(temp) <= death_zone_angle)
        {
            return YJ_OK;
        }
        // LOG(INFO) << "read_is_pos_ok: pos = " << pos << ", des_pos_temp = " << des_pos_temp << ", death_zone_angle = " << death_zone_angle << " temp: "<< temp << "timeout time ms:" << timeout_ms - yj_time_get_millitimestamp() + in_time;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    // LOG(INFO) << "read_zero_status: is_zero_ok = " << is_zero_ok << ", is_zero = " << is_zero;

    return YJ_ERROR;
}

yj_return zdt_motor::clear_protect()
{
    if (_rs485_flag != YJ_OK)
    {
        LOG(ERROR) << "clear_protect: zdt_motor init error";
        return YJ_ERROR;
    }

    std::vector<uint16_t> data_temp(1);
    data_temp[0] = 0x01;

    if (_modbus->write_holding_register_single(_id, 0x0E, data_temp) != 0)
    {
        return YJ_ERROR;
    }

    return YJ_OK;
}
